//------------------------------------------------------------------------------
//  <copyright file="ControllerStateMachineNativeTests.cpp" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
//  </copyright>
//------------------------------------------------------------------------------	

#include "stdafx.h"
#include "ControllerStateMachineNativeTests.h"

#include <math.h>

namespace ControllerStateMachineNativeTestsNS
{
	/// <summary>
	/// Adds two scalar values.
	/// parameters[0] = parameters[1] + parameters[2]
	/// <summary>
	/// <param name="parameters">
	/// Array of pointers to parameters for operation.
	/// parameters[0]:  Length 1, destination for operation result;
	/// parameters[1]:  Length 1.
	/// parameters[2]:  Length 1.
	/// </param>
	void Add(double **parameters)
	{
		*(parameters[0]) = *(parameters[1]) + *(parameters[2]);
	}

	/// <summary>
	/// Adds two vectors of length 3
	/// parameters[0] = parameters[1] + parameters[2]
	/// <summary>
	/// <param name="parameters">
	/// Array of pointers to parameters for operation.
	/// parameters[0]:  Length 3, destination for operation result;
	/// parameters[1]:  Length 3.
	/// parameters[2]:  Length 3.
	/// </param>
	void AddVector3(double **parameters)
	{
		int i;

		for(i = 0; i<3; i++)
		{
			*(parameters[0] + i) = *(parameters[1] + i) + *(parameters[2] + i);
		}
	}

	/// <summary>
	/// Multiplies two scalar values.
	/// parameters[0] = parameters[1] * parameters[2]
	/// <summary>
	/// <param name="parameters">
	/// Array of pointers to parameters for operation.
	/// parameters[0]:  Length 1, destination for operation result;
	/// parameters[1]:  Length 1.
	/// parameters[2]:  Length 1.
	/// </param>
	void Multiply(double **parameters)
	{
		*(parameters[0]) = (*(parameters[1])) * (*(parameters[2]));
	}

	/// <summary>
	/// Computes the dot product of two vectors of length 3.
	/// parameters[0] = parameters[1] dot parameters[2]
	/// <summary>
	/// <param name="parameters">
	/// Array of pointers to parameters for operation.
	/// parameters[0]:  Length 1, destination for operation result;
	/// parameters[1]:  Length 3.
	/// parameters[2]:  Length 3.
	/// </param>
	void DotProduct3(double **parameters)
	{
		int i;

		*(parameters[0]) = 0;

		for(i = 0; i<3; i++)
		{
			*(parameters[0]) += (*(parameters[1] + i)) * (*(parameters[2] + i));
		}
	}

	/// <summary>
	/// Calculates result of (parameter[1] > 1) and places result in parameters[0].
	/// <summary>
	/// <param name="parameters">
	/// Array of pointers to parameters for operation.
	/// parameters[0]:  Length 1, destination for operation result;
	/// parameters[1]:  Length 1.
	/// </param>
	void IsGreaterThanOne(double **parameters)
	{
		if(*(parameters[1]) > 1)
		{
			*(parameters[0]) = 1;
		}
		else
		{
			*(parameters[0]) = 0;
		}
	}

	/// <summary>
	/// Sets paramters[0] to 1 if the magnitude of parameter[1] is greater than paramter[2], otherwise
	/// sets parameter[0] to 0.
	/// <summary>
	/// <param name="parameters">
	/// Array of pointers to parameters for operation.
	/// parameters[0]:  Length 1, destination for operation result;
	/// parameters[1]:  Length 3.
	/// parameters[2]:  Length 1.
	/// </param>
	void IsMagnitudeGreaterThanVec3(double **parameters)
	{
		double sum = 0;
		int i;

		for (i = 0; i< 3; i++)
		{
			sum += (*(parameters[1] + i)) * (*(parameters[1] + i));
		}

		if (sqrt((sum)) > *(parameters[2]))
		{
			*parameters[0] = 1;
		}
		else
		{
			*parameters[0] = 0;
		}
	}

	/// <summary>
	/// Set a scalar value to 0
	/// parameters[0] = 0
	/// <summary>
	/// <param name="parameters">
	/// Array of pointers to parameters for operation.
	/// parameters[0]:  Length 1, destination for operation result;
	/// </param>
	void ZeroScalar(double **parameters)
	{
		*(parameters[0]) = 0;
	}

	void (*_ppOpPointers[])(double **) = {Add, Multiply, AddVector3, DotProduct3, IsGreaterThanOne, IsMagnitudeGreaterThanVec3, ZeroScalar};
	const int _numberOfOpPointers = 7;

	void ControllerStateMachineNativeTests::TestMachineCreation()
	{

		void (*ppLocalOpPointers[_numberOfOpPointers])(double **);

		for (int i = 0; i < _numberOfOpPointers; ++i)
		{
			ppLocalOpPointers[i] = _ppOpPointers[i];
		}

		int sd = sizeof(double);
		unsigned int pWorkingRegisterSizes[] = {1*sd, 1*sd, 1*sd, 3*sd, 3*sd, 3*sd};
		unsigned int pFeedbackRegisterSizes[] = {3*sd, 1*sd};

		int numWorkingRegisters = 6;

		int numFeedbackRegisters = 2;

		ControllerStateMachineRegisters<double> *pFeedbackRegisters =
			new ControllerStateMachineRegisters<double>(
			numFeedbackRegisters,
			pFeedbackRegisterSizes);

		ControllerStateMachineRegisters<double> *pWorkingRegisters = 
			new ControllerStateMachineRegisters<double>(
			numWorkingRegisters,
			pWorkingRegisterSizes);

		ControllerExecutionEngine<double> *pCee = new ControllerExecutionEngine<double>(
			pWorkingRegisters,
			pFeedbackRegisters,
			ppLocalOpPointers,
			CEE_VERSION);

		delete pCee;
		delete pWorkingRegisters;
		delete pFeedbackRegisters;
	}

	void ControllerStateMachineNativeTests::TestStateMachine()
	{
		void (*ppLocalOpPointers[_numberOfOpPointers])(double **);

		for (int i = 0; i < _numberOfOpPointers; ++i)
		{
			ppLocalOpPointers[i] = _ppOpPointers[i];
		}

		int sd = sizeof(double);
		unsigned int pWorkingRegisterSizes[] = {1*sd, 1*sd, 1*sd, 3*sd, 3*sd, 3*sd};
		unsigned int pFeedbackRegisterSizes[] = {3*sd, 1*sd};

		int numWorkingRegisters = 6;

		int numFeedbackRegisters = 2;

		ControllerStateMachineRegisters<double> *pFeedbackRegisters =
			new ControllerStateMachineRegisters<double>(
			numFeedbackRegisters,
			pFeedbackRegisterSizes);

		ControllerStateMachineRegisters<double> *pWorkingRegisters = 
			new ControllerStateMachineRegisters<double>(
			numWorkingRegisters,
			pWorkingRegisterSizes);

		ControllerExecutionEngine<double> *pCee = new ControllerExecutionEngine<double>(
			pWorkingRegisters,
			pFeedbackRegisters,
			ppLocalOpPointers,
			CEE_VERSION);

		ControllerStateMachineDescription csmd;

		csmd.NumStates = 3;

		// FIRST STATE
		csmd.State0.ControlMode = ControlModes::ControlMode1;
		csmd.State0.NumTransitions = 1;

		// Set up first transition
		// Target state of transition is 1.
		csmd.State0.Transition0.TargetState = 1;

		// No operations to be performed on transition
		csmd.State0.Transition0.Operations.NumOperations = 0;

		// Number of operands to test operation are 1 (results are not considered operands)
		// parameters[0] will be configured by machine, and test operation are expected to put
		// a 0 or 1 result here for no-transition/transition respectively
		csmd.State0.Transition0.NumTestOperands = 1;

		// We will test for a scalar register being greater than one here
		csmd.State0.Transition0.TestOpCode = OpCodes::IsGreaterThanOneOp;

		// We will test if scalar 1 is greater than zero
		csmd.State0.Transition0.TestOperand0.RegisterNumber = WorkingRegisters::Scalar1;
		csmd.State0.Transition0.TestOperand0.RegisterType = RegisterTypes::Working;

		// We will perform one operation in state 0
		csmd.State0.Operations.NumOperations = 1;

		csmd.State0.Operations.Operation0.OpCode = OpCodes::AddOp;

		csmd.State0.Operations.Operation0.NumOperands = 2;

		// we will increment the value in Scalar1 by Scalar2;
		csmd.State0.Operations.Operation0.Result.RegisterNumber = WorkingRegisters::Scalar1;
		csmd.State0.Operations.Operation0.Result.RegisterType = RegisterTypes::Working;
		csmd.State0.Operations.Operation0.Operand0.RegisterNumber = WorkingRegisters::Scalar1;
		csmd.State0.Operations.Operation0.Operand0.RegisterType = RegisterTypes::Working;
		csmd.State0.Operations.Operation0.Operand1.RegisterNumber = WorkingRegisters::Scalar2;
		csmd.State0.Operations.Operation0.Operand1.RegisterType = RegisterTypes::Working;

		double s1ExpectedValueAtStep1 = 0.5;
		double s1ExpectedValueAtStep2 = 1.0;
		double s1ExpectedValueAtStep4 = 0.5;
		double s1ExpectedValueAtStep5 = 1.0;
		double s1ExpectedValueAtStep6 = 0.5;

		double vec1ExpectedValueAtStep4 = 0.6;
		double vec1ExpectedValueAtStep5 = 0.6;
		double vec1ExpectedValueAtStep6 = 0.8;

		// SECOND STATE
		csmd.State1.ControlMode = ControlModes::ControlMode2;
		csmd.State1.NumTransitions = 2;

		// Set up first transition
		// Target state of transition is 0.
		csmd.State1.Transition0.TargetState = 0;

		// We will test for a scalar register being greater than one here
		csmd.State1.Transition0.TestOpCode = OpCodes::IsMagnitudeGreaterThanVec3Op;

		// We will test if the magnitude of Vec1 is greater than Scalar3
		csmd.State1.Transition0.TestOperand0.RegisterNumber = WorkingRegisters::Vec1;
		csmd.State1.Transition0.TestOperand0.RegisterType = RegisterTypes::Working;
		csmd.State1.Transition0.TestOperand1.RegisterNumber = WorkingRegisters::Scalar3;
		csmd.State1.Transition0.TestOperand1.RegisterType = RegisterTypes::Working;
		// Perfom one operation on transition
		csmd.State1.Transition0.Operations.NumOperations = 1;

		// Set Scalar1 to zero on transition (otherwise we'll be in an infinte loop)
		csmd.State1.Transition0.Operations.Operation0.NumOperands = 0;
		csmd.State1.Transition0.Operations.Operation0.OpCode = OpCodes::ZeroScalarOp;
		csmd.State1.Transition0.Operations.Operation0.Result.RegisterNumber = WorkingRegisters::Scalar1;
		csmd.State1.Transition0.Operations.Operation0.Result.RegisterType = RegisterTypes::Working;

		// Number of operands to test operation are 2 (results are not considered operands)
		// parameters[0] will be configured by machine, and test operation are expected to put
		// a 0 or 1 result here for no-transition/transition respectively
		csmd.State1.Transition0.NumTestOperands = 2;

		// Set up second transition
		// Target state of second transition is 2.
		csmd.State1.Transition1.TargetState = 2;

		// Number of operands to test operation are 1 (results are not considered operands)
		// parameters[0] will be configured by machine, and test operation are expected to put
		// a 0 or 1 result here for no-transition/transition respectively
		csmd.State1.Transition1.NumTestOperands = 1;

		// We will test for a scalar register being greater than one here
		csmd.State1.Transition1.TestOpCode = OpCodes::IsGreaterThanOneOp;

		// We will test if Sacalr 1 is greater than 1
		csmd.State1.Transition1.TestOperand0.RegisterNumber = WorkingRegisters::Scalar1;
		csmd.State1.Transition1.TestOperand0.RegisterType = RegisterTypes::Working;

		// Perfom three operations on transition
		csmd.State1.Transition1.Operations.NumOperations = 3;

		// Set Scalar1 to zero on transition 
		csmd.State1.Transition1.Operations.Operation0.NumOperands = 0;
		csmd.State1.Transition1.Operations.Operation0.OpCode = OpCodes::ZeroScalarOp;
		csmd.State1.Transition1.Operations.Operation0.Result.RegisterNumber = WorkingRegisters::Scalar1;
		csmd.State1.Transition1.Operations.Operation0.Result.RegisterType = RegisterTypes::Working;

		// Set Scalar2 = Scalar2 * Scalar 3 on transition
		csmd.State1.Transition1.Operations.Operation1.NumOperands = 2;
		csmd.State1.Transition1.Operations.Operation1.OpCode = OpCodes::MultiplyOp;
		csmd.State1.Transition1.Operations.Operation1.Result.RegisterNumber = WorkingRegisters::Scalar2;
		csmd.State1.Transition1.Operations.Operation1.Result.RegisterType = RegisterTypes::Working;
		csmd.State1.Transition1.Operations.Operation1.Operand0.RegisterNumber = WorkingRegisters::Scalar2;
		csmd.State1.Transition1.Operations.Operation1.Operand0.RegisterType = RegisterTypes::Working;
		csmd.State1.Transition1.Operations.Operation1.Operand1.RegisterNumber = WorkingRegisters::Scalar3;
		csmd.State1.Transition1.Operations.Operation1.Operand1.RegisterType = RegisterTypes::Working;

		// vec2 = vec2 + vec3 (should always be zero) on transition.
		csmd.State1.Transition1.Operations.Operation2.NumOperands = 2;
		csmd.State1.Transition1.Operations.Operation2.OpCode = OpCodes::AddVector3Op;
		csmd.State1.Transition1.Operations.Operation2.Result.RegisterNumber = WorkingRegisters::Vec2;
		csmd.State1.Transition1.Operations.Operation2.Result.RegisterType = RegisterTypes::Working;
		csmd.State1.Transition1.Operations.Operation2.Operand0.RegisterNumber = WorkingRegisters::Vec2;
		csmd.State1.Transition1.Operations.Operation2.Operand0.RegisterType = RegisterTypes::Working;
		csmd.State1.Transition1.Operations.Operation2.Operand1.RegisterNumber = WorkingRegisters::Vec3;
		csmd.State1.Transition1.Operations.Operation2.Operand1.RegisterType = RegisterTypes::Working;

		// We will perform one operation in state 1
		csmd.State1.Operations.NumOperations = 1;

		csmd.State1.Operations.Operation0.OpCode = OpCodes::AddVector3Op;

		csmd.State1.Operations.Operation0.NumOperands = 2;

		// vec1 = vec1 + vec2
		csmd.State1.Operations.Operation0.Result.RegisterNumber = WorkingRegisters::Vec1;
		csmd.State1.Operations.Operation0.Result.RegisterType = RegisterTypes::Working;
		csmd.State1.Operations.Operation0.Operand0.RegisterNumber = WorkingRegisters::Vec1;
		csmd.State1.Operations.Operation0.Operand0.RegisterType = RegisterTypes::Working;
		csmd.State1.Operations.Operation0.Operand1.RegisterNumber = WorkingRegisters::Vec2;
		csmd.State1.Operations.Operation0.Operand1.RegisterType = RegisterTypes::Working;

		double s1ExpectedValueAtStep3 = 0;
		double vec1ExpectedValueAtStep3 = 0.4;

		// THIRD STATE
		csmd.State2.ControlMode = ControlModes::ControlMode3;
		csmd.State2.NumTransitions = 1;

		// no operations
		csmd.State2.Operations.NumOperations = 0;

		// Set up first transition
		csmd.State2.Transition0.TargetState = 1;

		// No operations to be performed on transition
		csmd.State2.Transition0.Operations.NumOperations = 0;

		// Number of operands to test operation are 1 (results are not considered operands)
		// parameters[0] will be configured by machine, and test operation are expected to put
		// a 0 or 1 result here for no-transition/transition respectively
		csmd.State2.Transition0.NumTestOperands = 2;

		// We will test for the magnitude of a 3-vector being greater than a scalar
		csmd.State2.Transition0.TestOpCode = OpCodes::IsMagnitudeGreaterThanVec3Op;

		// This should always be true when we enter state 2, so we should never stop in it. 
		csmd.State2.Transition0.TestOperand0.RegisterNumber = WorkingRegisters::Vec1;
		csmd.State2.Transition0.TestOperand0.RegisterType = RegisterTypes::Working;
		csmd.State2.Transition0.TestOperand1.RegisterNumber = WorkingRegisters::Scalar1;
		csmd.State2.Transition0.TestOperand1.RegisterType = RegisterTypes::Working;

		// Set the machine as the current machine
		pCee->SetControllerStateMachineDescription(&csmd);

		//Set up Scalar2, Scalar3 and Vec2 so the above machine will do something interesting.

		RegisterID tempRegisterID;

		tempRegisterID.RegisterNumber = WorkingRegisters::Scalar2;
		tempRegisterID.RegisterType = RegisterTypes::Working;
		double* s2 = pCee->GetRegister(tempRegisterID);
		*s2 = 0.5;

		tempRegisterID.RegisterNumber = WorkingRegisters::Scalar3;
		tempRegisterID.RegisterType = RegisterTypes::Working;
		double *s3 = pCee->GetRegister(tempRegisterID);
		*s3 = 1;

		tempRegisterID.RegisterNumber = WorkingRegisters::Vec2;
		tempRegisterID.RegisterType = RegisterTypes::Working;
		double* vec2 = pCee->GetRegister(tempRegisterID);
		vec2[0] = 0.2;
		vec2[1] = 0.2;
		vec2[2] = 0.2;

		// Get pointers to Scalar1 and Vec1 to check machine operation
		tempRegisterID.RegisterNumber = WorkingRegisters::Scalar1;
		tempRegisterID.RegisterType = RegisterTypes::Working;
		double* s1 = pCee->GetRegister(tempRegisterID);

		tempRegisterID.RegisterNumber = WorkingRegisters::Vec1;
		tempRegisterID.RegisterType = RegisterTypes::Working;
		double* vec1 = pCee->GetRegister(tempRegisterID);
		vec1[0] = 0.0;
		vec1[1] = 0.0;
		vec1[2] = 0.0;

		int currentState;

		// start in state 0
		currentState = pCee->ExecuteOne();
		
		// Start in state 0
		// State 0 operation s1 = s1 + s2 is executed (0.5 = 0 + 0.5)
		// No transitions fire, so we shold remain in state 0, and S1 should = 0.5
		CONTROLLERSTATEMACHINENATIVEASSERT(0 == currentState, "Current state is 0");
		CONTROLLERSTATEMACHINENATIVEASSERT(abs(s1ExpectedValueAtStep1 - *s1) < ERROR_EPSILON, "S1 is 0.5");

		currentState = pCee->ExecuteOne();

		// Start in state 0
		// State 0 operation s1 = s1 + s2 is executed (1.0 = 0.5 + 0.5)
		// We should still remain in state zero, with Scalar1 = 1.0;
		CONTROLLERSTATEMACHINENATIVEASSERT(0 == currentState, "Current state is 0");
		CONTROLLERSTATEMACHINENATIVEASSERT(abs(s1ExpectedValueAtStep2 - *s1) < ERROR_EPSILON, "S1 is 1.0");

		currentState = pCee->ExecuteOne();

		// Start in state 0
		// State 0 operation s1 = s1 + s2 is executed (1.5 = 1.0 + 0.5)
		// Transition to state 1 now evalutates to true (s1 > 1, 1.5 > 1)
		// State 1 operation vec1 = vec1 + vec2 executes ([0.2, 0.2, 0.2] = [0, 0, 0] + [0.2, 0.2, 0.2])
		// Transition to state 2 evaluates to true (s1 > 1)
		// Three transition operations execute (s1 = 0, s2 = s2 * s3, vec2 = vec2 + vec3) resulting in (s1 = 0, s2 = 0.5, vec2 = [0.2, 0.2, 0.2])
		// No state operations to execute in state 2
		// Transition to state 1 evaluates true (|vec1| > s1, ~0.35 > 0), no operations on this transition
		// State 1 operation vec1 = vec1 + vec2 executes ([0.4, 0.4, 0.4] = [0.2, 0.2, 0.2] + [0.2, 0.2, 0.2])
		// No transitions from occur (both s1 > 1 and |vec1| > s3 evaluate false) and we stop in state 1
		// Scalar 1 should be zero, and vec1 = [.4,.4,.4] 
		CONTROLLERSTATEMACHINENATIVEASSERT(1 == currentState, "Current state is 1");
		CONTROLLERSTATEMACHINENATIVEASSERT(abs(s1ExpectedValueAtStep3 - *s1) < ERROR_EPSILON, "S1 is 0");

		CONTROLLERSTATEMACHINENATIVEASSERT(abs(vec1ExpectedValueAtStep3 - vec1[0]) < ERROR_EPSILON, "Vec1[0] is 0.4");
		CONTROLLERSTATEMACHINENATIVEASSERT(abs(vec1ExpectedValueAtStep3 - vec1[1]) < ERROR_EPSILON, "Vec1[1] is 0.4");
		CONTROLLERSTATEMACHINENATIVEASSERT(abs(vec1ExpectedValueAtStep3 - vec1[2]) < ERROR_EPSILON, "Vec1[2] is 0.4");

		currentState = pCee->ExecuteOne();

		// Start in state 1
		// State 1 operation vec1 = vec1 + vec2 executes ([0.6, 0.6, 0.6] = [0.4, 0.4, 0.4] + [0.2, 0.2, 0.2])
		// Transition to state 0 now evaluates to true (|vec1| > s3, ~1.04 > 1)
		// State 0 operation s1 = s1 + s2 executes (0.5 = 0 + 0.5)
		// No transitions out of state 0 evaluate to true
		// vec1 = [0.6, 0.6, 0.6], and scalar1 = 0.5
		CONTROLLERSTATEMACHINENATIVEASSERT(0 == currentState, "Current state is 0");
		CONTROLLERSTATEMACHINENATIVEASSERT(abs(s1ExpectedValueAtStep4 - *s1) < ERROR_EPSILON, "S1 is 0.5");

		CONTROLLERSTATEMACHINENATIVEASSERT(abs(vec1ExpectedValueAtStep4 - vec1[0]) < ERROR_EPSILON, "Vec1[0] is 0.6");
		CONTROLLERSTATEMACHINENATIVEASSERT(abs(vec1ExpectedValueAtStep4 - vec1[1]) < ERROR_EPSILON, "Vec1[1] is 0.6");
		CONTROLLERSTATEMACHINENATIVEASSERT(abs(vec1ExpectedValueAtStep4 - vec1[2]) < ERROR_EPSILON, "Vec1[2] is 0.6");

		currentState = pCee->ExecuteOne();

		// Start in state 0
		// State 0 operation s1 = s1 + s2 is executed (1.0 = 0.5 + 0.5)
		// We should still remain in state zero, with Scalar1 = 1.0;
		CONTROLLERSTATEMACHINENATIVEASSERT(0 == currentState, "Current state is 0");
		CONTROLLERSTATEMACHINENATIVEASSERT(abs(s1ExpectedValueAtStep5 - *s1) < ERROR_EPSILON, "S1 is 1.0");

		CONTROLLERSTATEMACHINENATIVEASSERT(abs(vec1ExpectedValueAtStep5 - vec1[0]) < ERROR_EPSILON, "Vec1[0] is 0.6");
		CONTROLLERSTATEMACHINENATIVEASSERT(abs(vec1ExpectedValueAtStep5 - vec1[1]) < ERROR_EPSILON, "Vec1[1] is 0.6");
		CONTROLLERSTATEMACHINENATIVEASSERT(abs(vec1ExpectedValueAtStep5 - vec1[2]) < ERROR_EPSILON, "Vec1[2] is 0.6");

		// Start in state 0
		// State 0 operation s1 = s1 + s2 is executed (1.5 = 1.0 + 0.5)
		// Transition to state 1 evaulates to true (s1 > 1, 1.5 > 1)
		// State 1 operation vec1 = vec1 + vec2 executes ([0.8, 0.8, 0.8] = [0.6, 0.6, 0.6] + [0.2, 0.2, 0.2])
		// Transition to state 0 (|vec1| > s3) evaluates true (takes precedences over transition to state 2 since it is first in the order)
		// Transition operation s1 = 0 executes
		// State 0 operation s1 = s1 + s2 is executed (0.5 = 0 + 0.5)
		// vec1 = [0.8, 0.8, 0.8], and scalar1 = 0.5
		currentState = pCee->ExecuteOne();
		CONTROLLERSTATEMACHINENATIVEASSERT(0 == currentState, "Current state is 0");
		CONTROLLERSTATEMACHINENATIVEASSERT(abs(s1ExpectedValueAtStep6 - *s1) < ERROR_EPSILON, "S1 is 0.5");

		CONTROLLERSTATEMACHINENATIVEASSERT(abs(vec1ExpectedValueAtStep6-vec1[0]) < ERROR_EPSILON, "Vec1[0] is 0.8");
		CONTROLLERSTATEMACHINENATIVEASSERT(abs(vec1ExpectedValueAtStep6-vec1[1]) < ERROR_EPSILON, "Vec1[1] is 0.8");
		CONTROLLERSTATEMACHINENATIVEASSERT(abs(vec1ExpectedValueAtStep6-vec1[2]) < ERROR_EPSILON, "Vec1[2] is 0.8");

		// Clean up
		delete pCee;
		delete pWorkingRegisters;
		delete pFeedbackRegisters;
	}

	ControllerStateMachineRegisters<double>  *ControllerStateMachineNativeTests::CreateDefaultControllerStateMachineRegisters()
	{
		unsigned int numRegisters = 10;
		unsigned int registerSizes[10] = 
		{
			1 * sizeof(double),
			2 * sizeof(double),
			100 * sizeof(double),
			3 * sizeof(double),
			1 * sizeof(double),
			5 * sizeof(double),
			9 * sizeof(double),
			2 * sizeof(double),
			1 * sizeof(double),
			2 * sizeof(double)
		};

		return new ControllerStateMachineRegisters<double>(
			numRegisters,
			registerSizes);
	}

	void ControllerStateMachineNativeTests::ControllerStateMachineRegistersCreation()
	{
		ControllerStateMachineRegisters<double> *pCsmr = 
			CreateDefaultControllerStateMachineRegisters();

		delete pCsmr;
	}

	void ControllerStateMachineNativeTests::ControllerStateMachineRegistersGetRegisterCount()
	{
		ControllerStateMachineRegisters<double> *pCsmr = 
			CreateDefaultControllerStateMachineRegisters();

		CONTROLLERSTATEMACHINENATIVEASSERT(10 == pCsmr->GetRegisterCount(), "Register count is 10");

		delete pCsmr;
	}

	void ControllerStateMachineNativeTests::ControllerStateMachineRegistersGetRegisterSize()
	{
		unsigned int registerSizes[10] = 
		{
			1 * sizeof(double),
			2 * sizeof(double),
			100 * sizeof(double),
			3 * sizeof(double),
			1 * sizeof(double),
			5 * sizeof(double),
			9 * sizeof(double),
			2 * sizeof(double),
			1 * sizeof(double),
			2 * sizeof(double)
		};

		ControllerStateMachineRegisters<double> *pCsmr = 
			CreateDefaultControllerStateMachineRegisters();

		for(int i = 0; i < 10; i++)
		{
			CONTROLLERSTATEMACHINENATIVEASSERT(registerSizes[i] == pCsmr->GetRegisterSizeInBytes(i), "Register size is correct");
		}

		delete pCsmr;
	}

	void ControllerStateMachineNativeTests::ControllerStateMachineRegistersGetRegisters()
	{
		ControllerStateMachineRegisters<double> *pCsmr = 
			CreateDefaultControllerStateMachineRegisters();

		double **ppAllRegisters = pCsmr->GetRegisters();

		double *registerFive = pCsmr->GetRegister(5);

		CONTROLLERSTATEMACHINENATIVEASSERT(0.0 == ppAllRegisters[5][3], "Register value is zero");

		registerFive[3] = 10.2;
		
		CONTROLLERSTATEMACHINENATIVEASSERT(10.2 == ppAllRegisters[5][3], "Register value is 10.2");

		delete pCsmr;
	}

	/// <summary> Test that state machine creation is error free. </summary>
	void TestMachineCreation()
	{
		ControllerStateMachineNativeTests::TestMachineCreation();
	}

	/// <summary> Test basic state machine functionality. </summary>
	void TestStateMachine()
	{
		ControllerStateMachineNativeTests::TestStateMachine();
	}

	/// <summary> Test that creation of controller state machine registers is error free. </summary.
	void ControllerStateMachineRegistersCreation()
	{
		ControllerStateMachineNativeTests::ControllerStateMachineRegistersCreation();
	}

	/// <summary> Test theat GetRegisterCount() works </summary>
	void ControllerStateMachineRegistersGetRegisterCount()
	{
		ControllerStateMachineNativeTests::ControllerStateMachineRegistersGetRegisterCount();
	}

	/// <summary> Test that GetRegisterSize() works. </summary>
	void ControllerStateMachineRegistersGetRegisterSize()
	{
		ControllerStateMachineNativeTests::ControllerStateMachineRegistersGetRegisterSize();
	}

	/// <summary> Test that GetRegisters() works. </summary>
	void ControllerStateMachineRegistersGetRegisters()
	{
		ControllerStateMachineNativeTests::ControllerStateMachineRegistersGetRegisters();
	}
}
